#include "my_task.h"

#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

#define TAG "MyTask"

MyTask::MyTask()
    : task_queue_(nullptr)
    , task_handle_(nullptr)
{
    task_queue_ = xQueueCreate(CONFIG_QUEUE_LEN, sizeof(Task*));
    if(!task_queue_) {
        ESP_LOGE(TAG, "Fail to create task queue.");
        return;
    }

    completion_sem_ = xSemaphoreCreateCounting(CONFIG_QUEUE_LEN, 0);
    if(!completion_sem_) {
        ESP_LOGE(TAG, "Fail to create completion semaphore.");
        vQueueDelete(task_queue_);
        task_queue_ = nullptr;
        return;
    }

    auto result = xTaskCreatePinnedToCore(
        [](void* arg) {
            auto* background = static_cast<MyTask*>(arg);
            background->TaskHandler();
        },
        "Background",
        CONFIG_STACK_SIZE,
        this,
        3,
        &task_handle_,
        CONFIG_CORE_ID == -1 ? tskNO_AFFINITY : CONFIG_CORE_ID
    );
    if(result != pdPASS) {
        ESP_LOGE(TAG, "Failed to create background task.");
        vQueueDelete(task_queue_);
        vSemaphoreDelete(completion_sem_);
        task_queue_ = nullptr;
        completion_sem_ = nullptr;
    }
}

MyTask::~MyTask()
{
    if(task_handle_) {
        vTaskDelete(task_handle_);
        task_handle_ = nullptr;
    }
    if(task_queue_) {
        Task* task = nullptr;
        while (xQueueReceive(task_queue_, &task, 0) == pdPASS) 
        {
            if(task) { 
                delete task;
            }
        }
        vQueueDelete(task_queue_);
        task_queue_ = nullptr;
    }
    if(completion_sem_) {
        vSemaphoreDelete(completion_sem_);
        completion_sem_ = nullptr;
    }
    ESP_LOGI(TAG, "Background task destroyed.");
}

bool MyTask::Schedule(TaskFunction fn, void* arg)
{
    if(!task_queue_) {
        return false;
    }
    if(uxQueueSpacesAvailable(task_queue_) == 0) {
        ESP_LOGW(TAG, "Task queue full.");
        return false;
    }

    auto* task = new Task(fn, arg);
    if(xQueueSend(task_queue_, &task, portMAX_DELAY) != pdPASS) {
        delete task;
        return false;
    }

    pending_tasks_ ++;
    return true;
}

bool MyTask::Schedule(std::function<void()> task)
{
    auto wrapper = new std::function<void()>(std::move(task));

    return Schedule(
        [](void* arg) {
            auto* func = static_cast<std::function<void()>*>(arg);
            (*func)();
            delete func;
        },
        wrapper
    );
}

bool MyTask::WaitForCompletion(uint32_t timeout_ms)
{
    if(!completion_sem_ || pending_tasks_ == 0) {
        return true;
    }

    size_t tasks_to_wait = pending_tasks_.load();

    // 计算超时时间
    auto timeout_ticks = timeout_ms == 0 ? portMAX_DELAY : pdMS_TO_TICKS(timeout_ms);
    
    // 等待任务完成
    for(size_t i = 0; i < pending_tasks_; i++) {
        if(xSemaphoreTake(completion_sem_, timeout_ticks) != pdPASS) {
            return false;
        }
    }
    return true;
}

size_t MyTask::QueuedTasks() const {
    if(!task_queue_) {
        return 0;
    }
    return uxQueueMessagesWaiting(task_queue_);
}

void MyTask::ClearQueue()
{
    if(!task_queue_) {
        return;
    }
    auto count = uxQueueMessagesWaiting(task_queue_);
    Task* task = nullptr;
    for(UBaseType_t i = 0; i < count; i++) {
        if(xQueueReceive(task_queue_, &task, 0) == pdPASS) {
            delete task;
            task =  nullptr;
            pending_tasks_ --;
            xSemaphoreGive(completion_sem_);
        }
    }
}

void MyTask::TaskHandler()
{
    Task* task = nullptr;
    while(true) {
        if(xQueueReceive(task_queue_, &task, portMAX_DELAY) != pdPASS) {
            continue;
        }

        if(task && task->fn) {
            task->fn(task->arg);
        }

        delete task;
        task = nullptr;
        pending_tasks_ --;

        if(completion_sem_) {
            xSemaphoreGive(completion_sem_);
        }

        vTaskDelay(pdMS_TO_TICKS(1));
    }
    ESP_LOGE(TAG, "Task Handler run out of range.");
    vTaskDelete(nullptr);
}